from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator
from nav2_msgs.action import FollowWaypoints  # 用于解析结果
import rclpy

def main():
    rclpy.init()
    nav = BasicNavigator()
    nav.waitUntilNav2Active()

    # 1. 定义目标点数据 [x, y, w]
    waypoints = [
        [2.0, 1.0, 1.0],    # 点1
        [0.0, 1.0, 1.0],     # 点2
        [0.0, 0.0, 1.0],     # 点3
    ]

    # 2. 自动生成 PoseStamped 列表
    goal_poses = []
    for wp in waypoints:
        pose = PoseStamped()
        pose.header.frame_id = "map"
        pose.header.stamp = nav.get_clock().now().to_msg()
        pose.pose.position.x = wp[0]
        pose.pose.position.y = wp[1]
        pose.pose.orientation.w = wp[2]

        goal_poses.append(pose)  #将当前生成的单个目标位姿(PoseStamped)添加到目标点列表(goal_poses)中

    # 3. 发送路点并监控状态
    nav.followWaypoints(goal_poses)  # 注意方法名！
    while not nav.isTaskComplete():
        feedback = nav.getFeedback()
        nav.get_logger().info(
            f'当前路点: {feedback.current_waypoint + 1}/{len(waypoints)}, '
        )

    # 4. 处理结果
    result = nav.getResult()
    nav.get_logger().info(f'导航结果：{result}')
    
    rclpy.shutdown()
